#include "StdAfx.h"
#include "OpenGLApp.h"
#include "geometry.h"
#define MAX_LOADSTRING 100
#include <algorithm>

using namespace NBE;
namespace banknamespace
{
	//////////////////////////////////////////////////////////////////////////
	

	// Global Variables:
	HINSTANCE hInst;								// current instance
	TCHAR szTitle[MAX_LOADSTRING];					// The title bar text
	TCHAR szWindowClass[MAX_LOADSTRING];			// the main window class name

	// Forward declarations of functions included in this code module:
	ATOM				MyRegisterClass(HINSTANCE hInstance);
	HWND				InitInstance(HINSTANCE, int);
	LRESULT CALLBACK	WndProc(HWND, UINT, WPARAM, LPARAM);
	INT_PTR CALLBACK	About(HWND, UINT, WPARAM, LPARAM);


	//
	//  FUNCTION: MyRegisterClass()
	//
	//  PURPOSE: Registers the window class.
	//
	//  COMMENTS:
	//
	//    This function and its usage are only necessary if you want this code
	//    to be compatible with Win32 systems prior to the 'RegisterClassEx'
	//    function that was added to Windows 95. It is important to call this function
	//    so that the application will get 'well formed' small icons associated
	//    with it.
	//
	ATOM MyRegisterClass(HINSTANCE hInstance)
	{
		WNDCLASSEX wcex;

		wcex.cbSize = sizeof(WNDCLASSEX);

		wcex.style			= CS_HREDRAW | CS_VREDRAW;
		wcex.lpfnWndProc	= WndProc;
		wcex.cbClsExtra		= 0;
		wcex.cbWndExtra		= 0;
		wcex.hInstance		= hInstance;
		wcex.hIcon			= LoadIcon(hInstance, MAKEINTRESOURCE(IDI_OPENGL_SHADER));
		wcex.hCursor		= LoadCursor(NULL, IDC_ARROW);
		wcex.hbrBackground	= 0; //(HBRUSH)(COLOR_WINDOW+1);
		wcex.lpszMenuName	= NULL; //MAKEINTRESOURCE(IDC_FINALPROJECT);
		wcex.lpszClassName	= szWindowClass;
		wcex.hIconSm		= LoadIcon(wcex.hInstance, MAKEINTRESOURCE(IDI_SMALL));

		return RegisterClassEx(&wcex);
	}

	//
	//   FUNCTION: InitInstance(HINSTANCE, int)
	//
	//   PURPOSE: Saves instance handle and creates main window
	//
	//   COMMENTS:
	//
	//        In this function, we save the instance handle in a global variable and
	//        create and display the main program window.
	//
	HWND InitInstance(HINSTANCE hInstance, int nCmdShow)
	{
	   HWND hWnd;

	   hInst = hInstance; // Store instance handle in our global variable

	   hWnd = CreateWindow(szWindowClass, szTitle, WS_OVERLAPPEDWINDOW,
		  CW_USEDEFAULT, 0, CW_USEDEFAULT, 0, NULL, NULL, hInstance, NULL);

	   if (!hWnd)
	   {
		  return FALSE;
	   }

	   ShowWindow(hWnd, nCmdShow);
	   UpdateWindow(hWnd);

	   return hWnd;
	}

	//
	//  FUNCTION: WndProc(HWND, UINT, WPARAM, LPARAM)
	//
	//  PURPOSE:  Processes messages for the main window.
	//
	//  WM_COMMAND	- process the application menu
	//  WM_PAINT	- Paint the main window
	//  WM_DESTROY	- post a quit message and return
	//
	//
	LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
	{
		int wmId, wmEvent;

		Mouse& mouse = GetTheMouse(hWnd);
		mouse.processMessages(hWnd, message, wParam, lParam);

		Keyboard& keyboard = GetTheKeyboard(hWnd);
		keyboard.processMessages(hWnd, message, wParam, lParam);

		switch (message)
		{
		case WM_COMMAND:
			wmId    = LOWORD(wParam);
			wmEvent = HIWORD(wParam);
			// Parse the menu selections:
			switch (wmId)
			{
			case IDM_ABOUT:
				DialogBox(hInst, MAKEINTRESOURCE(IDD_ABOUTBOX), hWnd, About);
				break;
			case IDM_EXIT:
				DestroyWindow(hWnd);
				break;
			default:
				return DefWindowProc(hWnd, message, wParam, lParam);
			}
			break;
		case WM_DESTROY:
			PostQuitMessage(0);
			break;
		default:
			return DefWindowProc(hWnd, message, wParam, lParam);
		}
		return 0;
	}

	// Message handler for about box.
	INT_PTR CALLBACK About(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
	{
		UNREFERENCED_PARAMETER(lParam);
		switch (message)
		{
		case WM_INITDIALOG:
			return (INT_PTR)TRUE;

		case WM_COMMAND:
			if (LOWORD(wParam) == IDOK || LOWORD(wParam) == IDCANCEL)
			{
				EndDialog(hDlg, LOWORD(wParam));
				return (INT_PTR)TRUE;
			}
			break;
		}
		return (INT_PTR)FALSE;
	}
	
	OpenGLApp::~OpenGLApp()
	{
		//delete m_model;
		//ShutdownOpenGL();
		ReleaseDC(m_hWnd, m_hdcWindow);
 
		clearBall();
	}
	
	OpenGLApp::OpenGLApp( HINSTANCE hInstance, int nCmdShow )
	{
		// fake
		// Initialize global strings
		LoadString(hInstance, IDS_APP_TITLE, szTitle, MAX_LOADSTRING);
		LoadString(hInstance, IDC_OPENGL_SHADER, szWindowClass, MAX_LOADSTRING);
		MyRegisterClass(hInstance);

		// Perform application initialization:
		m_hWnd = InitInstance(hInstance, nCmdShow);
		if (!m_hWnd)
		{
			throw InitFailed();
		}
		InputController::setMouseCenter(m_hWnd);
		m_hAccelTable = LoadAccelerators(hInstance, MAKEINTRESOURCE(IDC_OPENGL_SHADER));
		m_hdcWindow = GetDC( m_hWnd );

		//InitializeOpenGL();
		
		rendermode = "GL";

		rFac.init(rendermode);
		rFac.renderer->init( m_hWnd, m_hdcWindow );

		GetTheMouse(m_hWnd);
		GetTheKeyboard(m_hWnd);

		
		stoneid = rFac.renderer->loadTexture( "StoneTexture/cobblestonesDiffuse.bmp" );
		trollid = rFac.renderer->loadTexture( "StoneTexture/trollface.jpg" );
		defaultid = rFac.renderer->loadTexture( "StoneTexture/default.png" );

		m_camera.setPosition(vec3f(0,10,10));

		cubeSize = 5.0f;
		cube.setPosition(vec3f(0,12.5f,0));
		cube.updateMat();
		cube.actorState.m_prev.Mass = 128.0f;
		plain.buildCubePlain(cubeSize);
		cube.actorState.m_prev.Irb = Matrix3f(1/6.0f,0,0,
			0,1/6.0f,0,
			0,0,1/6.0f) * cube.actorState.m_prev.Mass * cubeSize * cubeSize;//(10*10 cube)
		cube.actorState.m_next = cube.actorState.m_prev;

		
		
		plainSize = 9999999.0f;

		plainActor.setPosition(vec3f(0,0,0));
		plainActor.updateMat();
		plainActor.actorState.m_prev.Mass = 9999999.0f;
		plainActor.actorState.m_prev.Irb = Matrix3f(1/6.0f,0,0,
			0,1/6.0f,0,
			0,0,1/6.0f) * plainActor.actorState.m_prev.Mass * plainSize * plainSize;//(10*10 cube)
		plainActor.actorState.m_next = plainActor.actorState.m_prev;
		
		
	}

	int OpenGLApp::RunMainLoop()
	{
		// Main loop:
		MSG msg;
			
		while (true)
		{
			if ( PeekMessage(&msg, NULL, 0, 0, PM_REMOVE ) )
			{
				if ( msg.message == WM_QUIT)
				{
					break;
				}

				if (!TranslateAccelerator(msg.hwnd, m_hAccelTable, &msg))
				{
					TranslateMessage(&msg);
					DispatchMessage(&msg);
				}
			}
			else
			{
				// Update the game frame
				
				UpdateFrame();
				GetTheMouse().update();
				GetTheKeyboard().update();
			}
		}

		return (int) msg.wParam;
	}


	void OpenGLApp::UpdateFrame()
	{
		// fake
		// Spin lock operations, limits to 60 frames per second
		{
			const static spacehax::SystemClocks CLOCKS_PER_FRAME = spacehax::SecondsToClocks( 1.0f / 60.0f);

			spacehax::SystemClocks now = spacehax::GetAbsoluteTimeClocks();
			while(now < nextFrameTime)
				now = spacehax::GetAbsoluteTimeClocks();

			nextFrameTime = now + CLOCKS_PER_FRAME;
		}

		rFac.renderer->clear();

		// calc aspect ratio
		RECT clientRect;
		::GetClientRect( m_hWnd, &clientRect );
		GLdouble aspectRatio = clientRect.right / static_cast< GLdouble > ( clientRect.bottom );

		//// setup scene-wide transformation
		rFac.renderer->setProjection(45.0f, (float)aspectRatio, 0.1f, 10000.0f);

		GLfloat projection[16];
		buildProjectionMatrix( projection, 45.0f, static_cast<float>(aspectRatio), 0.1f, 10000.0f );
		mat16f projectionMatrix( projection );

		if (::GetFocus() == m_hWnd)
		{
			bool isSpace = false;
			InputController::update( m_camera, m_camera, lightPos, lightPos2, stonecam);
		}
		m_camera.updateMat();
		m_camera.setViewTransformation();

		{
			rFac.renderer->loadMatrix((plainActor.actorMatrix * m_camera.actorMatrixInv).data());
			rFac.renderer->bindTexture(0, stoneid);	
			//renderSolidSquare(1000,0);
		}
		{
			rFac.renderer->loadMatrix(m_camera.actorMatrixInv.data());
			DrawOrigin(1000.0f);
		}
		
		if (ballVec.size()>40)
		{
			clearBall();
		}
		static unsigned int mode = 1;
		static bool applyGravity = false;
		if (IsKeyDown('1'))
		{
			mode = 1;
		}
		if (IsKeyDown('2'))
		{
			mode = 3;
			cube.setPosition(vec3f(0,12.5f,0));
			cube.updateMat();
			cube.actorState.m_prev.Vcm = vec3f();
			cube.actorState.m_prev.R = Matrix3f::Identity();

			cube.actorState.m_prev.Hi = vec3f();
			applyGravity = false;
		}
		if (IsKeyDown('3'))
		{
			mode = 2;
			cube.setPosition(vec3f(0,2.5f,0));
			cube.actorState.m_prev.Rcm = cube.position();
			cube.updateMat();
			cube.actorState.m_prev.Vcm = vec3f();
			cube.actorState.m_prev.R = Matrix3f::Identity(); 
			cube.actorState.m_prev.Hi = vec3f();
			applyGravity = false;
		}

 
		if (IsKeyDown('G'))
		{
			applyGravity = true;
		}
		if (IsKeyDown('H'))
		{
			applyGravity = false;
		}



		//shooting a ball
		if ( InputController::didMouseDown( 0 ) )
		{
			if(ballVec.size()>20)
			{
				delete (ballVec[0]);
				ballVec.erase(ballVec.begin());
			}
			addAball(0.2f);
		}

		unsigned int ballcount = ballVec.size();
 
  		std::vector<vec3f> tempNormalVec;
  		std::vector<vec3f> tempCollisionPoint;
		while(true)
		{
			
			vec3f collisionPoint;
			vec3f worldNormal;
			if(ballcount==0)break;
			Actor* ball = ballVec[ballcount-1];
			float plainSize = cubeSize/2.0f;
// 			if (!ball->isHit) 
// 			{
// 				ball->collideOnce = false;
// 				ball->setPosOnce = false;
// 			}

			float dotValue = 99999.0f;
			int selected = -1;
			
			if(mode == 2 || mode == 3 )
			{
				// plain detection
				for (unsigned int a = 0; a < plain.plainNormal.size(); ++a)
				{
					float dist = -1;

					vec4f translation = vec4f(cube.actorMatrix.data()[12],cube.actorMatrix.data()[13],cube.actorMatrix.data()[14]);
					vec4f plainPointInWorld4f[4];
					plainPointInWorld4f[0] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+0], 1.0f);// + translation;
					plainPointInWorld4f[1] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+1], 1.0f);// + translation;
					plainPointInWorld4f[2] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+2], 1.0f);// + translation;
					plainPointInWorld4f[3] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+3], 1.0f);// + translation;


					vec3f plainPointInWorld[4];
					for (int i = 0; i < 4; ++i)
					{
						for (int j = 0; j < 3; ++j)
						{
							plainPointInWorld[i][j] = plainPointInWorld4f[i][j];
						}
					}

					if (magic_testIntersectionSphereTriangle( ball->position(), ball->ballSize, plainPointInWorld[0], plainPointInWorld[1], plainPointInWorld[2], &dist))
					{
						ball->isHit = true;
						//ball.setVelocity(vec3f());

						if (ball->ballSize-dist < dotValue)
						{
							dotValue = ball->ballSize-dist;
							selected = a;
						}
					}

					if (magic_testIntersectionSphereTriangle( ball->position(), ball->ballSize, plainPointInWorld[2], plainPointInWorld[3], plainPointInWorld[0], &dist))
					{
						ball->isHit = true;
						//ball.setVelocity(vec3f());

						if (ball->ballSize-dist < dotValue)
						{
							dotValue = ball->ballSize-dist;
							selected = a;
						}
					}

				}

				if (selected > -1) 
				{
					vec4f tmp = cube.actorMatrixInv * vec4f(plain.plainNormal[selected]);// + translation;//normal is a direction no translation 
					worldNormal = vec3f(tmp.x, tmp.y, tmp.z);
					worldNormal.normalize();

					collisionPoint = ball->position() - (ball->ballSize-dotValue) * worldNormal;
					//ball.setPosition(vec3f(10000.0f, 10000.0f, 10000.0f));

					vec4f translation = vec4f(cube.actorMatrix.data()[12],cube.actorMatrix.data()[13],cube.actorMatrix.data()[14]);
					vec4f plainPointInWorld4f[4];
					plainPointInWorld4f[0] = cube.actorMatrix * vec4f(plain.plainPoint[selected*4+0], 1.0f);// + translation;
					plainPointInWorld4f[1] = cube.actorMatrix * vec4f(plain.plainPoint[selected*4+1], 1.0f);// + translation;
					plainPointInWorld4f[2] = cube.actorMatrix * vec4f(plain.plainPoint[selected*4+2], 1.0f);// + translation;
					plainPointInWorld4f[3] = cube.actorMatrix * vec4f(plain.plainPoint[selected*4+3], 1.0f);// + translation;

					float maxX, maxY, maxZ; maxX = -999999.0f; maxY = -999999.0f; maxZ = -999999.0f;
					float minX, minY, minZ; minX = 999999.0f; minY = 999999.0f; minZ = 999999.0f;
					for (int i = 0; i < 4; ++i)
					{
						if (plainPointInWorld4f[i].x > maxX) maxX = plainPointInWorld4f[i].x;
						if (plainPointInWorld4f[i].y > maxY) maxY = plainPointInWorld4f[i].y;
						if (plainPointInWorld4f[i].z > maxZ) maxZ = plainPointInWorld4f[i].z;

						if (plainPointInWorld4f[i].x < minX) minX = plainPointInWorld4f[i].x;
						if (plainPointInWorld4f[i].y < minY) minY = plainPointInWorld4f[i].y;
						if (plainPointInWorld4f[i].z < minZ) minZ = plainPointInWorld4f[i].z;
					}

					collisionPoint.x = clamp( collisionPoint.x, minX, maxX );
					collisionPoint.y = clamp( collisionPoint.y, minY, maxY );
					collisionPoint.z = clamp( collisionPoint.z, minZ, maxZ );

					//translation = vec4f(cube.actorMatrix.data()[12],cube.actorMatrix.data()[13],cube.actorMatrix.data()[14]);
					//vec4f tmp = cube.actorMatrixInv * vec4f(plain.plainNormal[selected]);// + translation;//normal is a direction no translation 
					//vec3f worldNormal(tmp.x, tmp.y, tmp.z);



					ball->actorState.m_prev.Rcm += ((collisionPoint - (ball->actorState.m_prev.Rcm - worldNormal*ball->ballSize))*worldNormal) * worldNormal ;

					ball->setPosition(ball->actorState.m_prev.Rcm); //TODO remove

					cube.solveCollision(collisionPoint - cube.actorState.m_prev.Rcm, collisionPoint - ball->actorState.m_prev.Rcm, -worldNormal,*ball,0.8f);	

				}
			}
			 if(applyGravity)
			 {
				 ball->applyForce(vec3f(),vec3f(0,-G * ball->actorState.m_prev.Mass,0));
			 }
			//ball vs plane
			if (ball->actorState.m_prev.Rcm.y - ball->ballSize <= plainActor.actorState.m_prev.Rcm.y)
			{
				ball->actorState.m_prev.Rcm.y = ball->ballSize + plainActor.actorState.m_prev.Rcm.y;
				ball->setPosition(ball->actorState.m_prev.Rcm);
				vec3f ballCollPoint(ball->actorState.m_prev.Rcm.x,ball->actorState.m_prev.Rcm.y,ball->actorState.m_prev.Rcm.z);
				ball->solveCollision(ballCollPoint - ball->actorState.m_prev.Rcm, ballCollPoint - plainActor.actorState.m_prev.Rcm,vec3f(0,1,0),plainActor,0.8f,true);
			}

			ball->updatePhysics(0.01f);
			rFac.renderer->loadMatrix((ball->actorMatrix * m_camera.actorMatrixInv).data());
			rFac.renderer->bindTexture(0, trollid);
			glLineWidth(1.0f);
			//renderWireSphere(ball->ballSize, 8, 8);
			renderSolidSphere(ball->ballSize, 8, 8);

			--ballcount;
			// 			tempNormalVec.push_back(worldNormal);
			// 			tempCollisionPoint.push_back(collisionPoint);
		}
		
// 		

// 			
		

		
		if(mode == 1 || mode == 2)
		{

			// cube/plain detection
			std::vector<vec3f> cubeCollisionPointsArr;
			std::vector<vec3f> cubePoints;


			for (unsigned int a = 0; a < plain.plainNormal.size(); ++a)
			{
				vec4f plainPointInWorld4f[4];
				plainPointInWorld4f[0] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+0], 1.0f);// + translation;
				plainPointInWorld4f[1] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+1], 1.0f);// + translation;
				plainPointInWorld4f[2] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+2], 1.0f);// + translation;
				plainPointInWorld4f[3] = cube.actorMatrix * vec4f(plain.plainPoint[a*4+3], 1.0f);// + translation;


				vec3f plainPointInWorld[4];
				for (int i = 0; i < 4; ++i)
				{
					for (int j = 0; j < 3; ++j)
					{
						plainPointInWorld[i][j] = plainPointInWorld4f[i][j];
					}

					if (plainPointInWorld[i].y <= plainActor.position().y)
					{
						// hit the ground
						bool found = false;
						for (unsigned int k = 0; k < cubeCollisionPointsArr.size(); ++k)
						{
							if (cubeCollisionPointsArr[k] == plainPointInWorld[i]) found = true;
						}
						if (!found)
						{
							cubeCollisionPointsArr.push_back(plainPointInWorld[i]);
						}
					}
					if (a == 0 || a == 3)
					{
						cubePoints.push_back(plainPointInWorld[i]);
					}
				}

			}

			if (cubeCollisionPointsArr.size() > 0)
			{

				float minY = 9999999.0f;
				for (unsigned int i = 0; i < cubeCollisionPointsArr.size(); ++i)
				{
					if (minY > cubeCollisionPointsArr[i].y)
					{
						minY = cubeCollisionPointsArr[i].y;
					}
				}

				if (minY != 9999999.0f)
				{
					cube.actorState.m_prev.Rcm -= vec3f(0,minY,0);
					//cube.actorState.m_prev.Rcm -= vec3f(0,-1,0);
					cube.setPosition(cube.actorState.m_prev.Rcm); //TODO remove
				}
				for (auto i = cubeCollisionPointsArr.begin(); i!=cubeCollisionPointsArr.end();)
				{
					(*i).y	-= minY;
					if((*i).y != 0)
						i = cubeCollisionPointsArr.erase(i);
					else
						++i;
				}
				for (auto i = cubePoints.begin();i!=cubePoints.end();++i)
				{
					(*i).y -= minY;
				}

				if(cubeCollisionPointsArr.size() ==4)
				{
					for (unsigned int i = 0; i < cubeCollisionPointsArr.size(); ++i)
					{
						cube.solveCollision(cubeCollisionPointsArr[i] - cube.actorState.m_prev.Rcm, cubeCollisionPointsArr[i] - vec3f(0,-plainSize/2,0), vec3f(0,1,0),plainActor,0.8f,true);	
						//	trace("Cube vel:"<<cube.actorState.m_prev.Vcm[0]<<","<<cube.actorState.m_prev.Vcm[1]<<","<<cube.actorState.m_prev.Vcm[2]<<std::endl);
					}
				}
				else if(cubeCollisionPointsArr.size() >= 1)
				{
					//cube.actorState.m_prev.Vcm.y *= -0.8f * cube.actorState.m_prev.Vcm.y;

					vec3f centerOfPoints;
					for (unsigned int i = 0; i<cubeCollisionPointsArr.size();++i)
					{
						centerOfPoints += cubeCollisionPointsArr[i];
						tempNormalVec.push_back(vec3f(0,1,0));
						tempCollisionPoint.push_back(cubeCollisionPointsArr[i]);

//  						for(unsigned int j = 0; j<cubePoints.size();++j){
//  							if(cubeCollisionPointsArr[i] != cubePoints[j])
//  							{
//  								cube.applyForce(cubePoints[j]- cube.actorState.m_prev.Rcm,vec3f(0,-G * cube.actorState.m_prev.Mass / 8 ,0));
//  							}else
//  							{
//  								cube.solveCollision(cubePoints[i] - cube.actorState.m_prev.Rcm, cubePoints[i] - vec3f(0,-plainSize/2,0),
//  									vec3f(0,1,0),plainActor,0.8f,true);	
//  							}
//  						}
					}
					centerOfPoints = centerOfPoints / cubeCollisionPointsArr.size();
					//cube.applyForce(cube.actorState.m_prev.Rcm - centerOfPoints,vec3f(0,-G * cube.actorState.m_prev.Mass  ,0));
					cube.solveCollision(vec3f(), centerOfPoints - vec3f(0,-plainSize/2,0), vec3f(0,1,0),plainActor,0.8f);	
				}

			}
			else
			{
				if(applyGravity)
					cube.applyForce(vec3f(),vec3f(0,-G * cube.actorState.m_prev.Mass ,0));
			}
		}
		//if (cube.position().y <= cubeSize/2)// size/2 //collision
		//{
		//	static float e = 0.9f;// collision of restitution
		//	cube.applyForce(vec3f(),vec3f(0,6,0));
		//	cube.actorState.m_prev.Vcm = e * ( cube.actorState.m_prev.Vcm) ;
		//	cube.actorState.m_prev.Vcm.y *=-1;	 
		//}
		cube.updatePhysics(0.01f);
		plainActor.updatePhysics(0.01f);
		//rFac.renderer->loadMatrix((cube.actorMatrix * m_camera.actorMatrixInv).data());
		rFac.renderer->loadMatrix((cube.actorMatrix * m_camera.actorMatrixInv).data());
		rFac.renderer->bindTexture(0, defaultid);
		renderSolidCube(cubeSize);

		//debug info

		for(auto i = 0;i <tempNormalVec.size();++i){
			rFac.renderer->loadMatrix(( m_camera.actorMatrixInv).data());
			glPushAttrib( GL_COLOR_BUFFER_BIT | GL_CURRENT_BIT | GL_DEPTH_BUFFER_BIT | GL_ENABLE_BIT );
			glDisable( GL_BLEND );
			glDisable( GL_TEXTURE_2D );
			glDisable( GL_DEPTH_TEST );
			glBegin(GL_LINES);
			glColor3f(1.0f, 1.0f, 0.0f);
			glVertex3f( tempCollisionPoint[i][0] ,  tempCollisionPoint[i][1] ,  tempCollisionPoint[i][2] );
			glVertex3f(tempCollisionPoint[i][0] + tempNormalVec[i][0]*10 ,  tempCollisionPoint[i][1] +tempNormalVec[i][1]*10,  tempCollisionPoint[i][2]+tempNormalVec[i][2]*10 ) ;
			glColor3f(1.0f, 1.0f, 1.0f);
			glEnd(); 
			glPopAttrib();
		}

		rFac.renderer->swapBuffer();
		++frameCounter;
	}

	Actor* OpenGLApp::addAball(float size)
	{
		Actor* ball = new Actor();
		ball->setPosition(m_camera.position());
		ball->actorState.m_prev.Rcm = ball->position();
		ball->m_drag = 0.0f;
		ball->isHit = false;
		ball->ballSize = size;
		
		ball->actorState.m_prev.Mass = 4 * 3.1415926f/3 * ball->ballSize * ball->ballSize * ball->ballSize * 125.0f;

		ball->actorState.m_prev.Irb = Matrix3f(2/5.0f,0,0,
			0,2/5.0f,0,
			0,0,2/5.0f) * ball->actorState.m_prev.Mass * ball->ballSize*ball->ballSize;
		ball->actorState.m_next = ball->actorState.m_prev;

		vec4f vel = m_camera.actorMatrixInv * vec4f(0.0f,0.0f,-0.1f,0.0f);
		ball->actorState.m_prev.Vcm = vec3f(vel.x, vel.y, vel.z);
		ball->actorState.m_prev.R = Matrix3f::Identity();
		ball->actorState.m_prev.w = vec3f();
		ball->actorState.m_prev.Hi = vec3f();

		ball->actorState.m_next.Vcm = vec3f();
		ball->actorState.m_next.R = Matrix3f::Identity();
		ball->actorState.m_next.w = vec3f();
		ball->actorState.m_next.Hi = vec3f();

		ballVec.push_back(ball);

		return ball;
	}

	void OpenGLApp::clearBall()
	{
		std::for_each(begin(ballVec),end(ballVec),[&](Actor* pActor){delete pActor;});
		ballVec.clear();
	}

}
